package model;

/*
 * Clase Singleton del Robot 
 * Contiene Información sobre el robot, velocidad y sensores. 
 * Esta clase es instanciada solo una vez. Sera convocada por los comportamientos. 
 * 
 * */

import lejos.nxt.Button;
import lejos.nxt.ColorSensor;
import lejos.nxt.ColorSensor.Color;
import lejos.nxt.LCD;
import lejos.nxt.SensorPort;
import lejos.nxt.comm.RConsole;
import lejos.robotics.RegulatedMotor;
import lejos.robotics.navigation.DifferentialPilot;
import lejos.robotics.navigation.RotateMoveController;
import lejos.robotics.subsumption.Arbitrator;
import lejos.robotics.subsumption.Behavior;
import lejos.util.PilotProps;

// SBOne Survillance Bot One.
public class SBOne{

	// Instancia del Singleton
	private static SBOne instance;
	
	private final static int LIGHT_LIMIT = 485;
	//PILOT_TRAVEL_SPEED es la velocidad del vehiculo en cm/s. 
	private final static double PILOT_TRAVEL_SPEED= 20; 

	private PilotProps pp;
	private float wheelDiameter;
	
	private float trackWidth;
	private RegulatedMotor leftMotor;
	private RegulatedMotor rightMotor;
	private boolean reverse;
	
	private RotateMoveController pilot;
	private static ColorSensor light; 
	private int direction =1;
	private boolean findDirection = false;
	int sweep = 45;
	long count_follow;
	
	
	public SBOne(){
		pp = new PilotProps();
		wheelDiameter = Float.parseFloat(pp.getProperty(PilotProps.KEY_WHEELDIAMETER, "4.0"));
		trackWidth = Float.parseFloat(pp.getProperty(PilotProps.KEY_TRACKWIDTH, "13.0"));
		leftMotor = PilotProps.getMotor(pp.getProperty(PilotProps.KEY_LEFTMOTOR, "A"));
		rightMotor = PilotProps.getMotor(pp.getProperty(PilotProps.KEY_RIGHTMOTOR, "C"));
		reverse = Boolean.parseBoolean(pp.getProperty(PilotProps.KEY_REVERSE,"false"));
		
		pilot = new DifferentialPilot(wheelDiameter, trackWidth, leftMotor, rightMotor, reverse);
		pilot.setTravelSpeed(PILOT_TRAVEL_SPEED);
    	pilot.setRotateSpeed(180);//180
    	light =  new ColorSensor(SensorPort.S1);
    	direction = 1; // 1 = right, -1 = left
    	count_follow = 0;
    	
	}
	
	/*Si la instancia no existe, se crea una instancia del Objeto SBOne, igual se retorna un instancia*/
	public static SBOne getInstance(){
		if(instance == null){
			instance = new SBOne();
		}
		return instance;		
	}
	
	public void stop(){
		pilot.stop();
	}
	
	public boolean isFindDirection(){
		return findDirection;
	}
	
	public void findDirection(){
		findDirection = true;
		pilot.travel(-3);
		pilot.rotate(23);
		
		if(light.getColorID() != Color.RED)
			pilot.rotate(-46);
		
		findDirection = false;
	}
	
	public void rotate(int angle, boolean immediateReturn) throws InterruptedException{
		
		sweep = 13 * direction;
//		findDirection();
		
//		pilot.stop();
//		Thread.yield();

//		Thread.sleep(200);
//		pilot.travel(-4.5, true);
//		Thread.sleep(1000);
//		pilot.stop();
//			Thread.sleep(2000);
//		
//		pilot.rotate(22, true);
//			Thread.sleep(1000);
//			
//		pilot.stop();	
//		while(true)
//			Thread.yield();
//
//		pilot.stop();
//		try {
//			Thread.sleep(2000);
//		} catch (InterruptedException e) {
//			// TODO Auto-generated catch block
//			e.printStackTrace();
//		}
//		
//		pilot.rotate(-2*(50), true);
//		try {
//			Thread.sleep(1000);
//		} catch (InterruptedException e) {
//			// TODO Auto-generated catch block
//			e.printStackTrace();
//		}
//		
//		pilot.stop();
//		while(true)
//			Thread.yield();
		
		while(light.getColorID() != Color.RED){	
//			RConsole.println("direction: " + direction  + "| sweep" + sweep );
			//Button.waitForPress();
			pilot.rotate(sweep, true);
			while(pilot.isMoving() && light.getColorID() != Color.RED){
			Thread.yield();
			
			}
			if(light.getColorID() != Color.RED)
			sweep *= -2;
			
			if(Math.abs(sweep) >= 46){
				pilot.travel(1.5);
				Thread.sleep(500);
				}
			if(Math.abs(sweep) >= 92){
				pilot.travel(2);
				Thread.sleep(200);
//				sweep*=-1;
				}
			
//			pilot.travel(-0.5);
//			try {
//				Thread.sleep(200);
//			} catch (InterruptedException e) {
//				// TODO Auto-generated catch block
//				e.printStackTrace();
//			}
				
		}	
				
	}
	


	
	public void forward(){
		count_follow =0;
		if(sweep < 0)
			direction = -1;
		else
			direction = 1;
		 pilot.forward();
		 while(light.getColorID() == Color.RED){
		Thread.yield();
		 count_follow ++;
		 }
		
	}
	
	public boolean isMoving(){
		return pilot.isMoving();
	}
	
	
	public int getColorID(){
		return light.getColorID();
	}
	
	public int getNormalizedLightValue(){
		return light.getNormalizedLightValue();
	}
	
	public RotateMoveController getPilot(){
		return pilot;
	}
	
	
}
